#include "ImageLandmarksVehicleWeightCalculator.h"

#include "utils.h"

using namespace	fvision;

CImageLandmarksVehicleWeightCalculator::CImageLandmarksVehicleWeightCalculator(
		LandmarkMap*                      pMap, 
		Camera1DInternal*                 pCamInternal,
		CObservationLikelihoodEvaluator*  pObservationLikelihoodEvaluator
	)
{
	this->pMap = pMap;
	this->pCamInternal = pCamInternal;
	this->pObservationLikelihoodEvaluator = pObservationLikelihoodEvaluator;
}

CImageLandmarksVehicleWeightCalculator::~CImageLandmarksVehicleWeightCalculator(void)
{
}

double CImageLandmarksVehicleWeightCalculator::compute(CParticleVehicleData* particle, CObservationImageLandmarks* pObservation) {

	Camera1DExternal camExternal(cvPoint2D32f(particle->x, particle->y), particle->w);

	Camera1D cam(*pCamInternal, camExternal);

	CObservationImageLandmarks predictedObservation = predictImageLandmarks(cam, *pMap);

	return pObservationLikelihoodEvaluator->eval(pObservation, &predictedObservation);
}